/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.assets.shooterstates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.assets.statemachine.State;
import edu.wpi.first.wpilibj.templates.assets.statemachine.StateMachine;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;

/**
 *
 * @author Jarrod Risley
 */
public class MoveUpOne extends State {

    /*
     * Global Class Variables
     */
    protected static MoveUpOne MOVE_UP = new MoveUpOne("Move Frizbee Up One");
    
    private boolean maxedOnBottomDiscs = false;
    private boolean maxedOnShooterDiscs = false;
    private boolean oldState;
    private int timesRun = 0;
    
    // CONSTRUCTOR //
    
    
    protected MoveUpOne(String name) {
        super(name);
    } // end MoveUpOne Constructor
    
    
    // METHODS //
    
    
    protected void runFirst() {
        
        setTimeoutTimer(RobotMap.MOVE_UP_ONE_TIME);
        System.out.println(this + " running.");
        
        oldState = CommandBase.shooter.checkSwitch(1);
        
    } // end runFirst

    protected void run() {
        
        
        
        if (maxedOnShooterDiscs) {
            
             System.out.println("Disc Count flag tripped!");
             setFinished();
             return;
             
        } else {
            
            if (timesRun < 2)
                CommandBase.shooter.moveUpOne();
            
            else {
                
                double start = Timer.getFPGATimestamp();
                
                do {
                    CommandBase.shooter.manualTopIndexerDrive(0.3);
                } while (Timer.getFPGATimestamp() > start + 0.30);
                
            } // end if-else
            
            if (isTimedOut())
                setFinished();
            
            System.out.println("Falling edge is: " + StateMachine.fallingEdge);
       if (StateMachine.fallingEdge == true)
           CommandBase.shooter.incrementShooterCount();
        } // end if-else
        
        
        
    } // end run

    protected void runLast() {
        
        
        
        if (CommandBase.shooter.getDiscCount() >= 2)
            maxedOnBottomDiscs = true;

        else if (CommandBase.shooter.getShooterCount() >= 2)
            maxedOnShooterDiscs = true;
        
        timesRun++;
        
    } // end runLast

    protected boolean isFinished() {
        return getFinished();
    } // end isFinished

    protected void end() {
        
        System.out.println(this + " ending.");
        CommandBase.shooter.stop();
        
    } // end end...lolz =P

    protected State getChainedInstance(State nextState) {
        
        MOVE_UP.setNext(nextState);
        return MOVE_UP;
        
    } // end getChainedInstance
    
    public void resetTimesRun() {
        timesRun = 0;
    } // end resetTimesRun
    
    public int getTimesRun() {
        return timesRun;
    } // end getTimesRun
    
} // end MoveUpOne
